
#pragma once

#include <memory>

#include <sensor_msgs/msg/point_cloud2.hpp>

// #include <rviz_default_plugins/displays/pointcloud/point_cloud_transport_display.hpp>
#include <rviz_common/display_context.hpp>

#include <rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp>
#include <rviz_default_plugins/visibility_control.hpp>
#include <rviz_common/display.hpp>
#include <rviz_common/properties/enum_property.hpp>
#include <rviz_common/properties/bool_property.hpp>
#include <rviz_common/properties/editable_enum_property.hpp>
#include <rviz_common/properties/tf_frame_property.hpp>
#include <rviz_common/config.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <rviz_common/ros_integration/ros_client_abstraction.hpp>
#include <rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp>
#include <rviz_common/properties/int_property.hpp>
#include "rviz_custom_plugins/utils/bag_map_loader.h"
#include "rviz_custom_plugins/utils/bag_map_notifier.h"
#include <QPointer>


// 需要运行这个命令添加tf树才能渲染出地图
// ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 world map

using rviz_common::Display;
using rviz_common::Config;
using rviz_default_plugins::PointCloudCommon;

namespace rviz_custom_plugins {

using utils::BagMapLoader;
using utils::BagMapNotifier;

namespace displays {


class RVIZ_DEFAULT_PLUGINS_PUBLIC BagMapDisplay : public Display
{
    Q_OBJECT

public:

    BagMapDisplay();
    ~BagMapDisplay() override;

    void load(const Config & config) override;
	void onInitialize() override;

	void reset() override;

  	void update(float wall_dt, float ros_dt) override;
	void onDisable() override;
    void setStatus(rviz_common::properties::StatusProperty::Level level, const QString & name, const QString & text) override;

private:
    rviz_common::properties::TfFrameProperty * frame_property_;
    rviz_common::properties::BoolProperty * selectable_property_;
	rviz_common::properties::IntProperty * size_property_;
	rviz_common::properties::IntProperty * point_size_property_;


	rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rosNode;

	std::unique_ptr<PointCloudCommon> point_cloud_common_;

	bool loadMap(QString& mapDir);

private:
    QPointer<BagMapLoader> mapLoader;
	BagMapNotifier* mapNotifier;

    pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclOriginPcdPoint;
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclPcdPoint;
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclPcdPointCache;

private Q_SLOTS:
    void handlePointCloudLoaded(pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdPointNormal);
    void handlePointCloudFiltered(pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdPointNormal);

    void handleProjectLoaded(QSharedPointer<QVariantMap> projectData);
    void handleProjectSaved(QSharedPointer<QVariantMap> projectData);
    void handleProjectClosed(QSharedPointer<QVariantMap> projectData);
};



}


}
